#include"ch32f103_flash.h"
#include"shell.h"
#include"ARM_CM3.h"
#include"dap_access.h"

#define ch32_FlashBlockSize     (0x400)
#define ch32_FlashLength        (0x10000)
#define ch32_FlashStart         (0x08000000)
#define ch32_FlashEnd           (ch32_FlashStart + ch32_FlashLength)
#define ch32_Ram_start          (0x20000000u)
#define ch32_RamLength          (20*1024)
extern unsigned long long DebugTick;
TargetFlash TargetFlash_t[3] = 
{
    {
        .Addr = 0x00000000,
        .Length = 0x08000000,
        .BlockSize = 0,
        .Type = RAM_Type,
    },
    {
        .Addr = ch32_FlashStart,
        .Length = ch32_FlashLength,
        .BlockSize = ch32_FlashBlockSize,
        .Type = Flash_Type,
    },
    {
        .Addr = 0x08010000,
        .Length = 0xf7ff0000,
        .BlockSize = 0,
        .Type = RAM_Type,
    }
};
static void ch32_STATR(void)
{
    unsigned int stat,Data;
    stat = AHB_Read(&(ch32flash->STATR),&Data,4);
    if(stat == 0)
    {
        user_printf("ch32flash->STATR 0x%x\r\n",Data);
    }
    else
    {
        user_printf("access error\r\n");
    }
}
SH_CMD(ch32_STATR,"ch32 flash STATR reg");
static void ch32_CTLR(void)
{
    unsigned int stat,Data;
    stat = AHB_Read(&(ch32flash->CTLR),&Data,4);
    if(stat == 0)
    {
        user_printf("ch32flash->CTLR 0x%x\r\n",Data);
    }
    else
    {
        user_printf("access error\r\n");
    }
}
SH_CMD(ch32_CTLR,"ch32 flash STATR reg");
static void ch32_WPR(void)
{
    unsigned int stat,Data;
    stat = AHB_Read(&(ch32flash->WPR),&Data,4);
    if(stat == 0)
    {
        user_printf("ch32flash->WPR 0x%x\r\n",Data);
    }
    else
    {
        user_printf("access error\r\n");
    }
}
SH_CMD(ch32_WPR,"ch32_WPR reg");
static void ch32_OBR(void)
{
    unsigned int stat,Data;
    stat = AHB_Read(&(ch32flash->OBR),&Data,4);
    if(stat == 0)
    {
        user_printf("ch32flash->OBR 0x%x\r\n",Data);
    }
    else
    {
        user_printf("access error\r\n");
    }
}
SH_CMD(ch32_OBR,"ch32_OBR reg");
#define ch32_FLASH_FLAG_BANK1_BSY       (0x00000001u)
#define ch32_FLASH_FLAG_BSY             (0x00000001u)
#define ch32_FLASH_FLAG_BANK1_PGERR     (0x00000004u)
#define ch32_FLASH_FLAG_BANK1_WRPRTERR  (0x00000010u)
#define ch32_FLASH_KEY1                 (0x45670123u)
#define ch32_FLASH_KEY2                 (0xCDEF89ABu)
#define ch32_EraseTimeout               (0x000B0000u)
#define ch32_ProgramTimeout             (0x00002000u)

#define ch32_CR_PG_Set                  (0x00000001u)
#define ch32_CR_PG_Reset                (0x00001FFEu) 
#define ch32_CR_PER_Set                 (0x00000002u)
#define ch32_CR_PER_Reset               (0x00001FFDu)
#define ch32_CR_MER_Set                 (0x00000004u)
#define ch32_CR_MER_Reset               (0x00001FFBu)
#define ch32_CR_OPTPG_Set               (0x00000010u)
#define ch32_CR_OPTPG_Reset             (0x00001FEFu)
#define ch32_CR_OPTER_Set               (0x00000020u)
#define ch32_CR_OPTER_Reset             (0x00001FDFu)
#define ch32_CR_STRT_Set                (0x00000040u)
#define ch32_CR_LOCK_Set                (0x00000080u)
unsigned int ch32_FLASH_GetBank1Status(void)
{
  unsigned int flashstatus = 0;
  unsigned int stat,data;
  stat = AHB_Read(&(ch32flash->STATR),&data,4);
  if(stat != 0)
  {
    flashstatus = 4;
    user_printf("ch32_FLASH_GetBank1Status error\r\n");
    goto close;
  }
  if((data & ch32_FLASH_FLAG_BANK1_BSY) == ch32_FLASH_FLAG_BSY) 
  {
    flashstatus = 1u;
  }
  else 
  {  
    if((data & ch32_FLASH_FLAG_BANK1_PGERR) != 0)
    { 
      flashstatus = 2;
    }
    else 
    {
      if((data & ch32_FLASH_FLAG_BANK1_WRPRTERR) != 0 )
      {
        flashstatus = 3u;
      }
      else
      {
        flashstatus = 0;
      }
    }
  }
  close:
  return flashstatus;
}
unsigned int ch32_FLASH_Lock(void)
{
    unsigned int stat;
    // FLASH->CTLR |= CR_LOCK_Set;
    stat = AHB_ReadWriteWord(&(ch32flash->CTLR),ch32_CR_LOCK_Set,opcode_OR);
    return stat;
}
unsigned int ch32_FLASH_Unlock(void)
{
    unsigned int stat1 = 0,stat2 = 0;
    stat1 = AHB_WriteWord(&(ch32flash->KEYR),ch32_FLASH_KEY1);
    stat2 = AHB_WriteWord(&(ch32flash->KEYR),ch32_FLASH_KEY2);
    return (stat1|stat2);
}
unsigned int ch32_FLASH_WaitForLastOperation(unsigned int Timeout)
{ 
  unsigned int status = 0;
   
  status = ch32_FLASH_GetBank1Status();
  while((status == 1) && (Timeout != 0x00))
  {
    status = ch32_FLASH_GetBank1Status();
    Timeout--;
  }
  if(Timeout == 0x00 )
  {
    status = 5;
  }
  return status;
}
unsigned int ch32_FLASH_EraseAllPages(void)
{
  unsigned int status = 0,stat,Data;

  status = FLASH_WaitForLastOperation(ch32_EraseTimeout);
  if(status == 0)
  {
    stat = AHB_Read(&(ch32flash->CTLR),&Data,4);
    if(stat != 0)
    {
        status = 1;
        goto close;
    }
    Data |= ch32_CR_MER_Set;
    stat = AHB_WriteWord(&(ch32flash->CTLR),Data);
    if(stat != 0)
    {
        status = 2;
        goto close;
    }
    
    //  FLASH->CTLR |= ch32_CR_STRT_Set;
    stat = AHB_Read(&(ch32flash->CTLR),&Data,4);
    if(stat != 0)
    {
        status = 3;
        goto close;
    }
    Data |= ch32_CR_STRT_Set;
    stat = AHB_WriteWord(&(ch32flash->CTLR),Data);
    if(stat != 0)
    {
        status = 4;
        goto close;
    }
    status = FLASH_WaitForLastOperation(ch32_EraseTimeout);

    // FLASH->CTLR &= ch32_CR_MER_Reset;
    stat = AHB_Read(&(ch32flash->CTLR),&Data,4);
    if(stat != 0)
    {
        status = 5;
        goto close;
    }
    Data &= ch32_CR_MER_Reset;
    stat = AHB_WriteWord(&(ch32flash->CTLR),Data);
    if(stat != 0)
    {
        status = 6;
        goto close;
    }
  }
    close:
  return status;
}
static void ch32_eraseall(void)
{
    unsigned int stat;
    stat = ch32_FLASH_Unlock();
    if(stat != 0)
    {
        user_printf("unlock failed 0x%x\r\n",stat);
        return;
    }
    stat = ch32_FLASH_EraseAllPages();
    if(stat == 0)
    {
        user_printf("erase all success\r\n");
    }
    else
    {
        user_printf("erase all faild\r\n");
    }
    stat = ch32_FLASH_Lock();
    if(stat == 0)
    {
        user_printf("flash lock success\r\n");
    }
    else
    {
        user_printf("flash lock faild\r\n");
    }
}
SH_CMD(ch32_eraseall,"ch32 all pages erase");
unsigned int ch32_FLASH_ErasePage(unsigned int Page_Address)
{
    unsigned int status = 0,stat,Data;
    status = ch32_FLASH_WaitForLastOperation(ch32_EraseTimeout);
  
    if(status == 0)
    { 
        // FLASH->CTLR|= CR_PER_Set;
        stat = AHB_Read(&(ch32flash->CTLR),&Data,4);
        if(stat != 0)
        {
            status = 1;
            goto close;
        }
        Data |= ch32_CR_PER_Set;
        stat = AHB_WriteWord(&(ch32flash->CTLR),Data);
        if(stat != 0)
        {
            status = 2;
            goto close;
        }

        // FLASH->ADDR = Page_Address;
        stat = AHB_WriteWord(&(ch32flash->ADDR),Page_Address);
        if(stat != 0)
        {
            status = 2;
            goto close;
        } 
        // FLASH->CTLR|= CR_STRT_Set;
        stat = AHB_Read(&(ch32flash->CTLR),&Data,4);
        if(stat != 0)
        {
            status = 1;
            goto close;
        }
        Data |= ch32_CR_STRT_Set;
        stat = AHB_WriteWord(&(ch32flash->CTLR),Data);
        if(stat != 0)
        {
            status = 2;
            goto close;
        }
        status = ch32_FLASH_WaitForLastOperation(ch32_EraseTimeout);

        // FLASH->CTLR &= CR_PER_Reset;
        stat = AHB_Read(&(ch32flash->CTLR),&Data,4);
        if(stat != 0)
        {
            status = 1;
            goto close;
        }
        Data &= ch32_CR_PER_Reset;
        stat = AHB_WriteWord(&(ch32flash->CTLR),Data);
        if(stat != 0)
        {
            status = 2;
            goto close;
        }
    }
    close:
    return status;
}
static void ch32_erase_sec(unsigned int argc,char*argv[])
{
    unsigned int stat,addr,data;
    if(argc < 2)
    {
        user_printf("input parameter less than 2\r\n");
    }
    else
    {
        sscanf(argv[1],"%x",&addr);
        stat = FlashIsLock(&data);
        if(stat != 0)
        {
            goto close;
        }
        if(data == 1)
        {
            stat = ch32_FLASH_Unlock();
            if(stat != 0)
            {
                user_printf("unlock failed 0x%x\r\n",stat);
                goto close;
            }
        }
        stat = ch32_FLASH_ErasePage(addr);
        if(stat == 0)
        {
            user_printf("Earea:0x%08x is earsed\r\n",addr);
        }
        else
        {
            user_printf("Earea:0x%08x is earsed failed\r\n",addr);
        }
        stat = ch32_FLASH_Lock();
        if(stat == 0)
        {
            user_printf("flash lock success\r\n");
        }
        else
        {
            user_printf("flash lock faild\r\n");
        }
    }
    close:
    return;
}
SH_CMD(ch32_erase_sec,"ch32 sector erase");
unsigned int ch32_FLASH_ProgramHalfWord(unsigned int Address, unsigned short Data)
{
  unsigned int status = 0,stat;

  status = ch32_FLASH_WaitForLastOperation(ch32_ProgramTimeout);
  if(status == 0)
  {
    // ch32flash->CTLR |= CR_PG_Set;
    stat = AHB_ReadWriteWord(&(ch32flash->CTLR),ch32_CR_PG_Set,opcode_OR);
    if(stat != 0)
    {
        status = 1;
        goto close;
    }
    // *(__IO uint16_t*)Address = Data;
    stat = AHB_WriteHalfWord(Address,Data);
    if(stat != 0)
    {
        status = 2;
        goto close;
    }
    status = ch32_FLASH_WaitForLastOperation(ch32_ProgramTimeout);
    // FLASH->CTLR &= CR_PG_Reset;
    stat = AHB_ReadWriteWord(&(ch32flash->CTLR),ch32_CR_PG_Reset,opcode_AND);
    if(stat != 0)
    {
        status = 1;
        goto close;
    }
  } 
  close:
  return status;
}
static void ch32_flash_w16(unsigned int argc,char*argv[])
{
    unsigned int stat,addr,Data;
    unsigned short data;
    if(argc < 3)
    {
        user_printf("input parameter less than 3\r\n");
    }
    else
    {
        sscanf(argv[1],"%x",&addr);
        stat = FlashIsLock(&Data);
        if(stat != 0)
        {
            goto close;
        }
        if(Data == 1)
        {
            stat = ch32_FLASH_Unlock();
            if(stat != 0)
            {
                user_printf("unlock failed 0x%x\r\n",stat);
                goto close;
            }
        }
        sscanf(argv[2],"%x",&data);
        stat = ch32_FLASH_ProgramHalfWord(addr,data);
        if(stat == 0)
        {
            user_printf("ch32 [0x%x] <= 0x%04x success\r\n",addr,data);
        }
        else
        {
            user_printf("ch32 [0x%x] <= 0x%04x faild\r\n",addr,data);
        }
    }
    close:
    return;
}
SH_CMD(ch32_flash_w16,"flash Write HalfWord");
unsigned int ch32_FlashErase(unsigned int Addr,unsigned int len)
{
    unsigned int BlockAddr_S,BlockAddr_E,BlockCount,i,ret,stat;
    BlockAddr_S = (Addr + ch32_FlashBlockSize - 1)&(~(ch32_FlashBlockSize - 1));
    BlockAddr_E = (Addr + len)&(~(ch32_FlashBlockSize - 1));
    if((BlockAddr_S >= ch32_FlashStart)&&(ch32_FlashEnd >=  BlockAddr_E))
    {
        stat = FlashIsLock(&i);
        if(stat != 0)
        {
            ret = 2;
            goto close;
        }
        if(i == 1)
        {
            stat = ch32_FLASH_Unlock();
            if(stat != 0)
            {
                ret = 2;
                goto close;
            }
        }
        BlockCount = (BlockAddr_E - BlockAddr_S)/ch32_FlashBlockSize;
        ret = 0;
        for(i = 0; i < BlockCount; i++)
        {
            stat = ch32_FLASH_ErasePage(BlockAddr_S + i*ch32_FlashBlockSize);
            if(stat != 0)
            {
                ret = 2;
                user_printf("Erase flash failed\r\n");
                break;
            }
        }
        stat = ch32_FLASH_Lock();
        if(stat == 0)
        {
            user_printf("flash lock success\r\n");
        }
        else
        {
            user_printf("flash lock faild\r\n");
        }
    }
    else
    {
        ret = 1;
    }
    close:
    return ret;
}
unsigned int FastFlashIsLock(unsigned int *res)
{
    unsigned int stat,ret=0,Data;
    stat = AHB_Read(&(ch32flash->CTLR),&Data,4);
    if(stat == 0)
    {
        *res = (Data >> 15)&1;
    }
    return stat;
}
unsigned int FlashIsLock(unsigned int *res)
{
    unsigned int stat,ret=0,Data;
    stat = AHB_Read(&(ch32flash->CTLR),&Data,4);
    if(stat == 0)
    {
        *res = (Data >> 7)&1;
    }
    return stat;

}
const char FlashWriteCode[]={
    #include"../contrib/loader/flash/ch32f103_2/ch32f10x_flash.inc"
};
unsigned int ch32_FlashProgram(unsigned int Addr,char*buf,unsigned int len)
{
    unsigned int ret = 0,stat,i,Data;
    unsigned short data;
    unsigned int cpu_reg[17]={0};
    stat = FlashIsLock(&Data);
    if(stat != 0)
    {
        ret = 2;
        goto close;
    }
    if(Data == 1)
    {
        stat = ch32_FLASH_Unlock();
        if(stat != 0)
        {
            ret = 2;
            goto close;
        }
    }
    //暂时不使用ram 写flash
#if 1    
    stat = AHB_BlockWrite(ch32_Ram_start,FlashWriteCode,sizeof(FlashWriteCode));
    if(stat != 0)
    {
        user_printf("Code Write failed\r\n");
        goto close;
    }
    cpu_reg[0]   = Addr;
    cpu_reg[1]   = ch32_Ram_start + 4096*2;//ch32_Ram_start+ sizeof(FlashWriteCode) +4096*2;
    cpu_reg[2]   = len;
    cpu_reg[3]   = ch32flash;
    cpu_reg[4]   = ch32_CR_PG_Reset;
    cpu_reg[5]   = 1 << 3;
    cpu_reg[6]   = 1 << 8;
    cpu_reg[13]  = ch32_Ram_start+ch32_RamLength -0X100;//sp
    cpu_reg[14] = ch32_Ram_start | 1;
    cpu_reg[15]  = ch32_Ram_start;
    cpu_reg[16]  = 0x01000000;//xpsr
    stat = AHB_BlockWrite(cpu_reg[1],buf,len);
    if(0 != stat)
    {
        goto dap_try_write;
    }
    stat = TargetRunCode(cpu_reg);
    if(stat == 0)
    {
        stat = WaitRAMCodeFinish(10);
        if(stat == 2)
        {
            user_printf("time out\r\n");

            HaltCPU();
            
            goto dap_try_write;
        }
        stat = GetRAMCodeReturn(cpu_reg);
        if(stat == 0)
        {
            user_printf("resust is 0x%08x\r\n",cpu_reg[0]);
        }
    }
    if(cpu_reg[0] == 0)
    {
        ret = 0;
        goto close;
    }
    dap_try_write:
#endif
    for(i = 0; i < len; i += 2)
    {
        data = buf[i+0] | (buf[i+1] << 8);
        stat = ch32_FLASH_ProgramHalfWord(Addr+i,data);
        if(stat != 0)
        {
            ret = 1;
            user_printf("ch32flash write [0x%x] <= 0x%x\r\n",Addr+i*2,data);
            break;
        }
    }
    stat = ch32_FLASH_Lock();
    if(stat == 0)
    {
        user_printf("flash lock success\r\n");
    }
    else
    {
        user_printf("flash lock faild\r\n");
    }
    close:
    return ret;
}

const char CopyCode[]={
    #include"../contrib/loader/copy/TargetDataCopy.inc"
};
static void target_copy(unsigned int argc,char*argv[])
{
    unsigned int srcaddr,stat,destaddr,len,Data;
    unsigned int cpu_reg[17]={0};
    unsigned long long timeout; 
    if(argc<4)
    {
        user_printf("input paratemer less than 4\r\n");
        goto close;
    }
    stat = AHB_BlockWrite(ch32_Ram_start,CopyCode,sizeof(CopyCode));
    if(stat != 0)
    {
        user_printf("Code Write failed\r\n");
        goto close;
    }
    destaddr = get_hex(argv[1]);
    srcaddr  = get_hex(argv[2]);
    len      = get_hex(argv[3]);
    cpu_reg[0]   = destaddr;
    cpu_reg[1]   = srcaddr;
    cpu_reg[2]   = len;
    cpu_reg[13]  = ch32_Ram_start+ch32_RamLength -4;//sp
    cpu_reg[15]  = ch32_Ram_start;
    cpu_reg[16]  = 0x01000000;//xpsr
    user_printf("destaddr 0x%08x\r\n",destaddr);
    user_printf("srcaddr  0x%08x\r\n",srcaddr);
    user_printf("len      0x%08x\r\n",len);
    user_printf("pc       0x%08x\r\n",cpu_reg[15]);
    user_printf("sp       0x%08x\r\n",cpu_reg[13]);
    stat = TargetRunCode(cpu_reg);
    if(stat != 0)
    {
        user_printf("Ram Run failed\r\n");
        goto close;
    }
    stat = WaitRAMCodeFinish(5);
    if(stat == 2)
    {
        user_printf("time out\r\n");
        goto close;
    }
    stat = GetRAMCodeReturn(cpu_reg);
    if(stat == 0)
    {
        user_printf("resust is 0x%08x\r\n",cpu_reg[0]);
    }
    close:
    user_printf("target_copy execulte finish\r\n");
    return;

}
SH_CMD(target_copy,"Target copy");
// FlagStatus FLASH_GetReadOutProtectionStatus(void)
// {
//   FlagStatus readoutstatus = RESET;
//   if ((FLASH->OBR & RDPRT_Mask) != (uint32_t)RESET)
//   {
//     readoutstatus = SET;
//   }
//   else
//   {
//     readoutstatus = RESET;
//   }
//   return readoutstatus;
// }
// FLASH_Status FLASH_EraseOptionBytes(void)
// {
//   uint16_t rdptmp = RDP_Key;

//   FLASH_Status status = FLASH_COMPLETE;
//   if(FLASH_GetReadOutProtectionStatus() != RESET)
//   {
//     rdptmp = 0x00;  
//   }
//   status = FLASH_WaitForLastOperation(EraseTimeout);
//   if(status == FLASH_COMPLETE)
//   {
//     FLASH->OBKEYR = FLASH_KEY1;
//     FLASH->OBKEYR = FLASH_KEY2;
    
//     FLASH->CTLR |= CR_OPTER_Set;
//     FLASH->CTLR |= CR_STRT_Set;
//     status = FLASH_WaitForLastOperation(EraseTimeout);
    
//     if(status == FLASH_COMPLETE)
//     {
//       FLASH->CTLR &= CR_OPTER_Reset;
//       FLASH->CTLR |= CR_OPTPG_Set;
//       OB->RDPR = (uint16_t)rdptmp; 
//       status = FLASH_WaitForLastOperation(ProgramTimeout);
 
//       if(status != FLASH_TIMEOUT)
//       {
//         FLASH->CTLR &= CR_OPTPG_Reset;
//       }
//     }
//     else
//     {
//       if (status != FLASH_TIMEOUT)
//       {
//         FLASH->CTLR &= CR_OPTPG_Reset;
//       }
//     }  
//   }
//   return status;
// }
// FLASH_Status FLASH_ErasePage(uint32_t Page_Address)
// {
//   FLASH_Status status = FLASH_COMPLETE;

//   status = FLASH_WaitForLastOperation(EraseTimeout);
  
//   if(status == FLASH_COMPLETE)
//   { 
//     FLASH->CTLR|= CR_PER_Set;
//     FLASH->ADDR = Page_Address; 
//     FLASH->CTLR|= CR_STRT_Set;
 
// 		status = FLASH_WaitForLastOperation(EraseTimeout);
    
//     FLASH->CTLR &= CR_PER_Reset;
//   }

//   return status;
// }